/* UnoJoy.c
 * Copyright (C) 2012 Alan Chatham <alan@openchord.org>
 * Made in conjunction with the RMIT Exertion Games Lab
 *
 * Edited by Nick Klaus. 2014
 *
 * Based on works by:
 *   Josh Kropf <http://git.slashdev.ca/ps3-teensy-hid>
 *   grunskis <http://github.com/grunskis/gamepad>
 *   Toodles <http://forums.shoryuken.com/showthread.php?t=131230>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/* Keyboard example for Teensy USB Development Board
 * http://www.pjrc.com/teensy/usb_keyboard.html
 * Copyright (c) 2008 PJRC.COM, LLC
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */


#include <avr/io.h>
#include <util/delay.h>
#include <avr/wdt.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>
#include "dataForController_t.h"
#include "usb_gamepad.h"

#define RXLED 4
#define TXLED 5

#define CPU_PRESCALE(n)	(CLKPR = 0x80, CLKPR = (n))

// This sets up an empty controller data packet and sends it out
//  to all the controllers attached.
void setControllersToZero(void){
	dataForController_t emptyData;
	emptyData.al0 = 128;
	emptyData.al1 = 128;
	emptyData.al2 = 128;
	emptyData.al3 = 128;
	emptyData.al4 = 128;
	emptyData.al5 = 128;
	emptyData.al6 = 128;
	emptyData.al7 = 128;
	
	emptyData.al8 = 128;
	emptyData.al9 = 128;	
	emptyData.al10 = 128;
	emptyData.al11 = 128;
	emptyData.al12 = 128;
	emptyData.al13 = 128;
	emptyData.al14 = 128;
	emptyData.al15 = 128;
	
	emptyData.al16 = 128;
	emptyData.al17 = 128;
	emptyData.al18 = 128;
	emptyData.al19 = 128;
	emptyData.al20 = 128;
	emptyData.al21 = 128;
	emptyData.al22 = 128;
	emptyData.al23 = 128;
	
	emptyData.al24 = 128;
	emptyData.al25 = 128;
	emptyData.al26 = 128;
	emptyData.al27 = 128;
	emptyData.al28 = 128;
	emptyData.al29 = 128;
	emptyData.al30 = 128;
	emptyData.al31 = 128;
	
	emptyData.al32 = 128;
	emptyData.al33 = 128;
	emptyData.al34 = 128;
	emptyData.al35 = 128;
	emptyData.al36 = 128;
	emptyData.al37 = 128;
	emptyData.al38 = 128;
	emptyData.al39 = 128;
	
	emptyData.al40 = 128;
	emptyData.al41 = 128;
	emptyData.al42 = 128;
	emptyData.al43 = 128;
	emptyData.al44 = 128;
	emptyData.al45 = 128;
	emptyData.al46 = 128;
	emptyData.al47 = 128;
	
	sendPS3Data(emptyData);
}

// Initializes the USART to receive and transmit,
//  takes in a value you can find in the datasheet
//  based on desired communication and clock speeds
void USART_Init(uint16_t baudSetting){
	// Set baud rate
	UBRR1 = baudSetting;
	// Enable receiver and transmitter
	UCSR1B = (1<<RXEN1)|(1<<TXEN1);
	// Set frame format: 8data, 1stop bit
	UCSR1C = (1<<UCSZ10)|(1<<UCSZ11);	
}

// This reads the USART serial port, returning any data that's in the
//  buffer, or a guaranteed zero if it took longer than timeout ms
//  Input: uint_16 timeout - milliseconds to wait for data before timing out
unsigned char serialRead( uint16_t timeout ){
	// Wait for data to be received 
	while ( !(UCSR1A & (1<<RXC1)) ){
		_delay_ms(1);
		timeout--;
		if (timeout == 0){
			return 0b0;
		}			
	}	
	// Get and return received data from buffer 
	return UDR1;
}

// This sends out a byte of data via the USART.
void serialWrite( unsigned char data )
{
	// Wait for empty transmit buffer
	while ( !( UCSR1A & (1<<UDRE1)) ){
	}	
	// Put data into buffer, sends the data
	UDR1 = data;
}

void flushSerialRead()
{
	unsigned char dummy;
	while ( UCSR1A & (1<<RXC1) )
		dummy = UDR1;
}

// This turns on one of the LEDs hooked up to the chip
void LEDon(char ledNumber){
	DDRD |= 1 << ledNumber;
	PORTD &= ~(1 << ledNumber);
}

// And this turns it off
void LEDoff(char ledNumber){
	DDRD &= ~(1 << ledNumber);
	PORTD |= 1 << ledNumber;
}

int main(void) {
	// Make sure our watchdog timer is disabled!
	wdt_reset(); 
	MCUSR &= ~(1 << WDRF); 
	wdt_disable();

	// Start up the USART for serial communications
	// 25 corresponds to 38400 baud - see datasheet for more values
	USART_Init(25);// 103 corresponds to 9600, 8 corresponds to 115200 baud, 3 for 250000
	
	// set the prescale for the USB for our 16 MHz clock
	CPU_PRESCALE(0);

	// Initialize our USB connection
	usb_init();
	while (!usb_configured()){
		LEDon(TXLED);
		_delay_ms(50);
		LEDoff(TXLED);
		_delay_ms(50);
	} // wait

	// Wait an extra second for the PC's operating system to load drivers
	// and do whatever it does to actually be ready for input
	// This wait also gives the Arduino bootloader time to timeout,
	//  so the serial data you'll be properly aligned.
	_delay_ms(500);
	dataForController_t dataToSend;
	char buttonData1;
	char buttonData2;
	char buttonData3;
	char buttonData4;
	char buttonData5;
	char buttonData6;	
	char buttonData7;
	char buttonData8;
	char buttonData9;
	char buttonData10;
	char buttonData11;
	char buttonData12;
	char buttonData13;
	char buttonData14;
	char buttonData15;
	char buttonData16;	


	while (1) {
		// Delay so we're not going too fast
		_delay_ms(10);
		
        // We get our data from the ATmega328p by writing which byte we
        //  want from the dataForController_t, and then wait for the
        //  ATmega328p to send that back to us.
        // The serialRead(number) function reads the serial port, and the
        //  number is a timeout (in ms) so if there's a transmission error,
        //  we don't stall forever.
		LEDon(TXLED);
		flushSerialRead();
		serialWrite(0);
		buttonData1 = serialRead(25);
		       
		serialWrite(1);
		buttonData2 = serialRead(25);
        
		serialWrite(2);
		buttonData3 = serialRead(25);
		
   		serialWrite(3);
		buttonData4 = serialRead(25);
		       
		serialWrite(4);
		buttonData5 = serialRead(25);
        
		serialWrite(5);
		buttonData6 = serialRead(25);
		
		serialWrite(6);
		buttonData7 = serialRead(25);
		
		serialWrite(7);
		buttonData8 = serialRead(25);
		
		serialWrite(8);
		buttonData9 = serialRead(25);		

		serialWrite(9);
		buttonData10 = serialRead(25);
		
		serialWrite(10);
		buttonData11 = serialRead(25);
		
		serialWrite(11);
		buttonData12 = serialRead(25);
		
		serialWrite(12);
		buttonData13 = serialRead(25);
		
		serialWrite(13);
		buttonData14 = serialRead(25);
		
		serialWrite(14);
		buttonData15 = serialRead(25);
		
		serialWrite(15);
		buttonData16 = serialRead(25);
			    
		serialWrite(16);
		dataToSend.al0 = serialRead(25);
        
		serialWrite(17);
		dataToSend.al1 = serialRead(25);
        
		serialWrite(18);
		dataToSend.al2 = serialRead(25);
        
		serialWrite(19);
		dataToSend.al3= serialRead(25);

		serialWrite(20);
		dataToSend.al4 = serialRead(25);
		
		serialWrite(21);
		dataToSend.al5 = serialRead(25);
		
		serialWrite(22);
		dataToSend.al6 = serialRead(25);
		
		serialWrite(23);
		dataToSend.al7= serialRead(25);
		
		serialWrite(24);
		dataToSend.al8 = serialRead(25);
		
		serialWrite(25);
		dataToSend.al9 = serialRead(25);
		
		serialWrite(26);
		dataToSend.al20 = serialRead(25);
		
		serialWrite(27);
		dataToSend.al21= serialRead(25);		

		serialWrite(28);
		dataToSend.al22 = serialRead(25);
		
		serialWrite(29);
		dataToSend.al23= serialRead(25);

		serialWrite(30);
		dataToSend.al24 = serialRead(25);
		
		serialWrite(31);
		dataToSend.al25 = serialRead(25);
		
		serialWrite(32);
		dataToSend.al26 = serialRead(25);
		
		serialWrite(33);
		dataToSend.al27= serialRead(25);
		
		serialWrite(34);
		dataToSend.al28 = serialRead(25);
		
		serialWrite(35);
		dataToSend.al29 = serialRead(25);
		
		serialWrite(36);
		dataToSend.al30 = serialRead(25);
		
		serialWrite(37);
		dataToSend.al31= serialRead(25);
				
		serialWrite(38);
		dataToSend.al32 = serialRead(25);
		
		serialWrite(39);
		dataToSend.al33= serialRead(25);

		serialWrite(40);
		dataToSend.al34 = serialRead(25);
		
		serialWrite(41);
		dataToSend.al35 = serialRead(25);
		
		serialWrite(42);
		dataToSend.al36 = serialRead(25);
		
		serialWrite(43);
		dataToSend.al37= serialRead(25);
		
		serialWrite(44);
		dataToSend.al38 = serialRead(25);
		
		serialWrite(45);
		dataToSend.al39 = serialRead(25);
		
		serialWrite(46);
		dataToSend.al40 = serialRead(25);
		
		serialWrite(47);
		dataToSend.al41= serialRead(25);

		serialWrite(48);
		dataToSend.al42 = serialRead(25);
		
		serialWrite(49);
		dataToSend.al43= serialRead(25);

		serialWrite(50);
		dataToSend.al44 = serialRead(25);
		
		serialWrite(51);
		dataToSend.al45 = serialRead(25);
		
		serialWrite(52);
		dataToSend.al46 = serialRead(25);
		
		serialWrite(53);
		dataToSend.al47= serialRead(25);

		
		LEDoff(TXLED);
		
		// Now, we take the button data we got in and input that information
        //  into our controller data we want to send
		dataToSend.bt0On = 1 & (buttonData1 >> 0);
		dataToSend.bt1On = 1 & (buttonData1 >> 1);
		dataToSend.bt2On = 1 & (buttonData1 >> 2);
		dataToSend.bt3On = 1 & (buttonData1 >> 3);
		dataToSend.bt4On = 1 & (buttonData1 >> 4);
		dataToSend.bt5On = 1 & (buttonData1 >> 5);
		dataToSend.bt6On = 1 & (buttonData1 >> 6);
		dataToSend.bt7On = 1 & (buttonData1 >> 7);
		
		dataToSend.bt8On = 1 & (buttonData2 >> 0);
		dataToSend.bt9On = 1 & (buttonData2 >> 1);
		dataToSend.bt10On = 1 & (buttonData2 >> 2);
		dataToSend.bt11On = 1 & (buttonData2 >> 3);
		dataToSend.bt12On = 1 & (buttonData2 >> 4);
		dataToSend.bt13On = 1 & (buttonData2 >> 5);
		dataToSend.bt14On = 1 & (buttonData2 >> 6);
		dataToSend.bt15On = 1 & (buttonData2 >> 7);
		
		dataToSend.bt16On = 1 & (buttonData3 >> 0);
		dataToSend.bt17On = 1 & (buttonData3 >> 1);
		dataToSend.bt18On = 1 & (buttonData3 >> 2);
		dataToSend.bt19On = 1 & (buttonData3 >> 3);
		dataToSend.bt20On = 1 & (buttonData3 >> 4);
		dataToSend.bt21On = 1 & (buttonData3 >> 5);
		dataToSend.bt22On = 1 & (buttonData3 >> 6);
		dataToSend.bt23On = 1 & (buttonData3 >> 7);
		
		dataToSend.bt24On = 1 & (buttonData4 >> 0);
		dataToSend.bt25On = 1 & (buttonData4 >> 1);
		dataToSend.bt26On = 1 & (buttonData4 >> 2);
		dataToSend.bt27On = 1 & (buttonData4 >> 3);
		dataToSend.bt28On = 1 & (buttonData4 >> 4);
		dataToSend.bt29On = 1 & (buttonData4 >> 5);
		dataToSend.bt30On = 1 & (buttonData4 >> 6);
		dataToSend.bt31On = 1 & (buttonData4 >> 7);		

		dataToSend.bt32On = 1 & (buttonData5 >> 0);
		dataToSend.bt33On = 1 & (buttonData5 >> 1);
		dataToSend.bt34On = 1 & (buttonData5 >> 2);
		dataToSend.bt35On = 1 & (buttonData5 >> 3);
		dataToSend.bt36On = 1 & (buttonData5 >> 4);
		dataToSend.bt37On = 1 & (buttonData5 >> 5);
		dataToSend.bt38On = 1 & (buttonData5 >> 6);
		dataToSend.bt39On = 1 & (buttonData5 >> 7);
		
		dataToSend.bt40On = 1 & (buttonData6 >> 0);
		dataToSend.bt41On = 1 & (buttonData6 >> 1);
		dataToSend.bt42On = 1 & (buttonData6 >> 2);
		dataToSend.bt43On = 1 & (buttonData6 >> 3);
		dataToSend.bt44On = 1 & (buttonData6 >> 4);
		dataToSend.bt45On = 1 & (buttonData6 >> 5);
		dataToSend.bt46On = 1 & (buttonData6 >> 6);
		dataToSend.bt47On = 1 & (buttonData6 >> 7);		

		dataToSend.bt48On = 1 & (buttonData7 >> 0);
		dataToSend.bt49On = 1 & (buttonData7 >> 1);
		dataToSend.bt50On = 1 & (buttonData7 >> 2);
		dataToSend.bt51On = 1 & (buttonData7 >> 3);
		dataToSend.bt52On = 1 & (buttonData7 >> 4);
		dataToSend.bt53On = 1 & (buttonData7 >> 5);
		dataToSend.bt54On = 1 & (buttonData7 >> 6);
		dataToSend.bt55On = 1 & (buttonData7 >> 7);		

		dataToSend.bt56On = 1 & (buttonData8 >> 0);
		dataToSend.bt57On = 1 & (buttonData8 >> 1);
		dataToSend.bt58On = 1 & (buttonData8 >> 2);
		dataToSend.bt59On = 1 & (buttonData8 >> 3);
		dataToSend.bt60On = 1 & (buttonData8 >> 4);
		dataToSend.bt61On = 1 & (buttonData8 >> 5);
		dataToSend.bt62On = 1 & (buttonData8 >> 6);
		dataToSend.bt63On = 1 & (buttonData8 >> 7);
		
		dataToSend.bt64On = 1 & (buttonData9 >> 0);
		dataToSend.bt65On = 1 & (buttonData9 >> 1);
		dataToSend.bt66On = 1 & (buttonData9 >> 2);
		dataToSend.bt67On = 1 & (buttonData9 >> 3);
		dataToSend.bt68On = 1 & (buttonData9 >> 4);
		dataToSend.bt69On = 1 & (buttonData9 >> 5);
		dataToSend.bt70On = 1 & (buttonData9 >> 6);
		dataToSend.bt71On = 1 & (buttonData9 >> 7);
		
		dataToSend.bt72On = 1 & (buttonData10 >> 0);
		dataToSend.bt73On = 1 & (buttonData10 >> 1);
		dataToSend.bt74On = 1 & (buttonData10 >> 2);
		dataToSend.bt75On = 1 & (buttonData10 >> 3);
		dataToSend.bt76On = 1 & (buttonData10 >> 4);
		dataToSend.bt77On = 1 & (buttonData10 >> 5);
		dataToSend.bt78On = 1 & (buttonData10 >> 6);
		dataToSend.bt79On = 1 & (buttonData10 >> 7);						

		dataToSend.bt80On = 1 & (buttonData11 >> 0);
		dataToSend.bt81On = 1 & (buttonData11 >> 1);
		dataToSend.bt82On = 1 & (buttonData11 >> 2);
		dataToSend.bt83On = 1 & (buttonData11 >> 3);
		dataToSend.bt84On = 1 & (buttonData11 >> 4);
		dataToSend.bt85On = 1 & (buttonData11 >> 5);
		dataToSend.bt86On = 1 & (buttonData11 >> 6);
		dataToSend.bt87On = 1 & (buttonData11 >> 7);
		
		dataToSend.bt88On = 1 & (buttonData12 >> 0);
		dataToSend.bt89On = 1 & (buttonData12 >> 1);
		dataToSend.bt90On = 1 & (buttonData12 >> 2);
		dataToSend.bt91On = 1 & (buttonData12 >> 3);
		dataToSend.bt92On = 1 & (buttonData12 >> 4);
		dataToSend.bt93On = 1 & (buttonData12 >> 5);
		dataToSend.bt94On = 1 & (buttonData12 >> 6);
		dataToSend.bt95On = 1 & (buttonData12 >> 7);
		
		dataToSend.bt96On = 1 & (buttonData13 >> 0);
		dataToSend.bt97On = 1 & (buttonData13 >> 1);
		dataToSend.bt98On = 1 & (buttonData13 >> 2);
		dataToSend.bt99On = 1 & (buttonData13 >> 3);
		dataToSend.bt100On = 1 & (buttonData13 >> 4);
		dataToSend.bt101On = 1 & (buttonData13 >> 5);
		dataToSend.bt102On = 1 & (buttonData13 >> 6);
		dataToSend.bt103On = 1 & (buttonData13 >> 7);		

		dataToSend.bt104On = 1 & (buttonData14 >> 0);
		dataToSend.bt105On = 1 & (buttonData14 >> 1);
		dataToSend.bt106On = 1 & (buttonData14 >> 2);
		dataToSend.bt107On = 1 & (buttonData14 >> 3);
		dataToSend.bt108On = 1 & (buttonData14 >> 4);
		dataToSend.bt109On = 1 & (buttonData14 >> 5);
		dataToSend.bt110On = 1 & (buttonData14 >> 6);
		dataToSend.bt111On = 1 & (buttonData14 >> 7);

		dataToSend.bt112On = 1 & (buttonData15 >> 0);
		dataToSend.bt113On = 1 & (buttonData15 >> 1);
		dataToSend.bt114On = 1 & (buttonData15 >> 2);
		dataToSend.bt115On = 1 & (buttonData15 >> 3);
		dataToSend.bt116On = 1 & (buttonData15 >> 4);
		dataToSend.bt117On = 1 & (buttonData15 >> 5);
		dataToSend.bt118On = 1 & (buttonData15 >> 6);
		dataToSend.bt119On = 1 & (buttonData15 >> 7);

		dataToSend.bt120On = 1 & (buttonData16 >> 0);
		dataToSend.bt121On = 1 & (buttonData16 >> 1);
		dataToSend.bt122On = 1 & (buttonData16 >> 2);
		dataToSend.bt123On = 1 & (buttonData16 >> 3);
		dataToSend.bt124On = 1 & (buttonData16 >> 4);
		dataToSend.bt125On = 1 & (buttonData16 >> 5);
		dataToSend.bt126On = 1 & (buttonData16 >> 6);
		dataToSend.bt127On = 1 & (buttonData16 >> 7);
        
        // Finally, we send the data out via the USB port
		sendPS3Data(dataToSend);	
		
	}
}